/*
 * @Author: Jiagang Chen
 * @Date: 2021-11-11 13:52:35
 * @LastEditors: Jiagang Chen
 * @LastEditTime: 2021-12-01 03:05:32
 * @Description: ...
 * @Reference: ...
 */
//
// Created by tao on 7/11/21.
//

#include "kaist_publisher/altimeter_converter.h"

namespace kaist_publisher {

AltimeterConverter::AltimeterConverter(ros::NodeHandle &nh,
                                       const std::string &dataset_dir,
                                       const std::string &save_dir,
                                       const std::string &topic)
    : Converter(dataset_dir, save_dir), topic_(topic) {
    bag_name_ = FilterSlash(topic_) + ".bag";
    pub_altimeter_ = nh.advertise<lci_slam::altimeter>(topic_.c_str(), 10);
}

int64_t AltimeterConverter::Init_Publish() {
    const std::string data_file = dataset_dir_ + "/" + default_data_file;
    fp_ = fopen(data_file.c_str(), "r");

    if (fscanf(fp_, "%ld,%lf\n", &stamp_, &altimeter_value_) == 2) {
        return stamp_;
    } else {
        fclose(fp_);
        return LONG_MAX;
    }
}

int64_t AltimeterConverter::Publish() {
    lci_slam::altimeter altimeter_data;
    altimeter_data.header.stamp.fromNSec(stamp_);
    altimeter_data.header.frame_id = "altimeter";
    altimeter_data.data = altimeter_value_;
    pub_altimeter_.publish(altimeter_data);

    if (fscanf(fp_, "%ld,%lf\n", &stamp_, &altimeter_value_) == 2) {
        return stamp_;
    }

    fclose(fp_);
    return LONG_MAX;
}

void AltimeterConverter::Publish(int64_t &timestamp,
                                 lci_slam::altimeterPtr &altimeter_data) {
    altimeter_data.reset(new lci_slam::altimeter());
    altimeter_data->header.stamp.fromNSec(stamp_);
    altimeter_data->header.frame_id = "altimeter";
    altimeter_data->data = altimeter_value_;
    // pub_altimeter_.publish(altimeter_data);

    if (fscanf(fp_, "%ld,%lf\n", &stamp_, &altimeter_value_) == 2) {
        timestamp = stamp_;
        return;
    }

    fclose(fp_);
    timestamp = LONG_MAX;
    return;
}

int AltimeterConverter::Convert() {
    CheckAndCreateSaveDir();

    boost::filesystem::path bag_file =
        boost::filesystem::path(save_dir_) / bag_name_;
    // rosbag::Bag bag(bag_file.string(), rosbag::bagmode::Write);
    // ROS_INFO("saving %s", bag_file.c_str());

    const std::string data_file = dataset_dir_ + "/" + default_data_file;
    FILE *fp = fopen(data_file.c_str(), "r");
    lci_slam::altimeter altimeter_data;
    int64_t stamp;
    double altimeter_value;
    while (fscanf(fp, "%ld,%lf\n", &stamp, &altimeter_value) == 2) {
        altimeter_data.header.stamp.fromNSec(stamp);
        altimeter_data.header.frame_id = "altimeter";
        altimeter_data.data = altimeter_value;

        // bag.write(topic_, altimeter_data.header.stamp, altimeter_data);
        pub_altimeter_.publish(altimeter_data);
    }
    // bag.close();
    fclose(fp);
    // ROS_INFO("done saving %s", bag_file.c_str());

    return 0;
}

}  // namespace kaist_publisher